﻿using System;
using System.Linq;
using COC.Character;
using COC.Exception;
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Random = UnityEngine.Random;

namespace COC.Tools.AI.Motion
{

    public abstract class ISteeringDriver
    {

        private static SteeringDriverImpl mm;
        public static List<AIBehaveAware> list = new List<AIBehaveAware>();//boid input a list into.
        private int nextAI;
        private CMoveType mType;
        public readonly float MAX_VELOCITY = 35.0f;//maximun velocity
        public readonly static float MAX_FORCE = 0.4f;
        public readonly static float SLOW_RADIUS = .2f;
        public readonly static float FLEE_RADIUS = 1;
        #region following the path
        public readonly static float MIN_DIST_TO_PATH_POINT = 6;//how close to get to point in path before considered reached
        public readonly static float MAX_TIME_TO_SPEND_ON_PATH_POINT = 10;
        private readonly static float MINDISTANCETOTARGETTOSTARTSEEK = 5;
        #endregion
        private float MASS = 20;//max speed for steering froce.
        #region wander proprietary
        public readonly static float WANDERNG_CIRCLE_DISTANCE = .6f;
        private readonly static float WANDERNG_CIRCLE_RADIUS = .5f;
        private readonly static float WANDERING_ANGLE_CHANGE = 10;
        #endregion
        #region collision avoidance
        public readonly static float MAX_AVOID_AHED = 1f;
        public readonly static float AVOID_FORCE = 2f;
        public readonly static float MAX_OBSTACLE_RADIUS = .5f;
        public readonly List<Vector3> obstacles = new List<Vector3>();
        #endregion
        #region queue properties
        public readonly static float MAX_QUEUE_AHEAD = 1f;
        public readonly static float MAX_QUEUE_RADIUS = 3.5f;
        #endregion
        #region follow leader
        public readonly static float LEADER_BEHIND_DIST = 1.3f;
        public readonly static float LEADER_SIGHT_RADIUS = 2f;
        public readonly static float SLOWINGRADIUS = 2.2f;
        #endregion
        #region separation proprietary
        public readonly static float SEPARATION_RADIUS = 2.5f;
        public readonly static float MAX_SEPARATION = 3;
        #endregion
        //private delegate void SteeringBehavior(params object[] args);
        // private SteeringBehavior mBehavior;
        public static SteeringDriverImpl getInstance()
        {
            if (mm == null)
                mm = new SteeringDriverImpl();
            return mm;
        }

        protected internal List<AIBehaveAware> getAllAvailableAI()
        {
            return list;
        } 
        protected internal int bindAgent<T>(AIBehaveAware mode)
        {

            list.Add(mode);
            nextAI++;//next ai will be inputed on list.
            return nextAI - 1;//get new item id after add into list.

        }
        protected internal AIBehaveAware pickupNeighborAI()
        {
            return list[nextAI - 1];
        }
        protected internal void registObstacles(Vector3 obstacle)
        {
            obstacles.Add(obstacle);
        }
        protected internal void unbindAgent(AIBehaveAware mode)
        {

            if (list.Contains(mode))
            {
                list.Remove(mode);
                //TODO send a notification
            }
            else
            {
                throw new CException("we could not find this object,did you include that when beginning to move action?");
            }
            nextAI--;
        }
        protected internal void dispose(AIBehaveAware t)
        {
            unbindAgent(t);
        }
        protected internal int count()
        {

            return list.Count;
        }
        /* public void switchType(CMoveType type)
         {
             mBehavior += null;
             switch (type)
             {
                 case CMoveType.SEEK:
                     mBehavior += seek;
                     break;
                 case CMoveType.FLEE:
                     mBehavior += flee;
                     break;
                 case CMoveType.PURSUE:
                     mBehavior += pursue;
                     break;
                 case CMoveType.EVADE:
                     mBehavior += evade;
                     break;
                 case CMoveType.FOLLOW_PATH:
                     mBehavior += followingPath;
                     break;
                 case CMoveType.QUEUE:
                     mBehavior += queue;
                     break;
                 case CMoveType.ARRIVAL:
                     mBehavior += arrival;
                     break;
           
             }
             mType = type;
         }*/

        public virtual Vector3 seek(Vector3 agentPos, Vector3 targetPos)
        {
            return Vector3.zero;
        }
        /// <summary>
        /// steering and froce together as a smooth path.
        /// </summary>
        /// <param name="agentPos"></param>
        /// <param name="targetPos"></param>
        /// <param name="velocity"></param>
        /// <returns></returns>
        public virtual Vector3 seekPath(AIBehaveAware imove)
        {
            return Vector3.zero;
        }
        /// <summary>
        /// flee behavior seem like away from target or reverse to target position.
        /// </summary>
        /// <param name="imove"></param>
        /// <returns></returns>
        public virtual Vector3 flee(AIBehaveAware imove)
        {
            return Vector3.zero;
        }

        public virtual Vector3 pursuit(AIBehaveAware imove, AIBehaveAware neighborAI)
        {
            return Vector3.zero;
        }

        public virtual Vector3 evade(AIBehaveAware imove, AIBehaveAware targetAI)
        {
            return Vector3.zero;
        }

        public virtual Vector3 followingPath(AIBehaveAware imove, Vector3[] path)
        {
            return Vector3.zero;
        }
        public virtual Vector3 queue(AIBehaveAware imove)
        {
            return Vector3.zero;
        }
        public virtual Vector3 arrival(AIBehaveAware imove)
        {

            return Vector3.zero;
        }

        public virtual Vector3 wander(AIBehaveAware imove)
        {
            return Vector3.zero;
        }

        public virtual Vector3 collisonAvoidance(AIBehaveAware imove)
        {
            return Vector2.zero;
        }

        /// <summary>
        /// adjust center of position of a bolt.
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        public virtual Vector3 adjustVector3(AIBehaveAware pos)
        {

            return Vector3.zero;
        }
        public virtual Vector3 separation(AIBehaveAware imove)
        {
            return Vector3.zero;
        }

        public virtual Vector3 followLeader(AIBehaveAware imove, AIBehaveAware leader)
        {
            return Vector3.zero;
        }
        public enum CMoveType
        {
            NONE = 0x0,//no dehavior
            SEEK = 0x1,//seek behavior
            FLEE = 0x2,//flee behavior
            PURSUE = 0x3,//persue behavior
            EVADE = 0x04,//evade or away from targetPos position
            FOLLOW_PATH = 0x5,//following a targetPos path on it.
            QUEUE = 0x6,//participating to join a queue
            ARRIVAL = 0x7//something like agentPos is closed to targetPos position.

        }

        public class SteeringDriverImpl : ISteeringDriver
        {


            public override Vector3 seek(Vector3 agentPos, Vector3 targetPos)
            {
               // Debug.Log("seek:" + Vector3.Normalize(targetPos - agentPos) * MAX_VELOCITY);
                return Vector3.Normalize(targetPos - agentPos) * MAX_VELOCITY;//calculating steering
            }
            public override Vector3 seekPath(AIBehaveAware imove)
            {
                Vector3 steering = Vector3.zero;
                steering = seek(imove.agentPos, imove.targetPos);//calculating steering
                steering -= imove.velocity;//adding force
                truncate(steering, MAX_FORCE);//adjust velocity in order to constraint this velocity
                steering *= 1 / MASS;
                imove.velocity += steering;
                UnityEngine.Debug.DrawLine(imove.agentPos, imove.velocity.normalized, Color.blue);
                truncate(imove.velocity, imove.max_velocity);
                return imove.velocity;
            }
            private void truncate(Vector3 velocity, float max)
            {
                if (velocity.magnitude > max)
                {
                    velocity.Normalize(); // Vector3.normalized returns this vector with a magnitude of 1
                    velocity *= max; //scale to max
                }

            }
            public override Vector3 flee(AIBehaveAware imove)
            {
                float distance = Vector3.Distance(imove.agentPos, imove.targetPos);
                Vector3 steering = Vector3.zero;
                if (distance < FLEE_RADIUS)
                {
                    steering = (imove.agentPos - imove.targetPos).normalized *  distance/FLEE_RADIUS;
                   #if UNITY_EDITOR
                    UnityEngine.Debug.Log("steering:" + steering);
                    #endif
                }
                /* else
                 {
                     steering = -seek(imove);//calculating steering
                     steering -= imove.velocity;//adding force
                     truncate(steering, MAX_FORCE);//adjust velocity in order to constraint this velocity
                     steering *= 1 / MASS;
                     steering+=imove.velocity;
                     truncate(steering, MAX_VELOCITY);
                 }*/
                return steering;
            }

            public override Vector3 pursuit(AIBehaveAware imove, AIBehaveAware neighborAI)
            {
                Vector3 steering = Vector3.zero;
                AIBehaveAware neighbor = neighborAI;//pick up a randomizing velocity that nearly far from this fouced ai,then estimate a velocity that whether pursue target position exactly.   
                AIBehaveAware futureTargetPosition = neighbor;//travel of future forwarding target position.
                float distance = Vector3.Distance(imove.targetPos, futureTargetPosition.agentPos);
                futureTargetPosition.velocity *= (int)(distance / imove.max_velocity);//if distance / MAX_VELOCITY too short,this result very close to target position,otherwise far away from target position.
                futureTargetPosition.agentPos += futureTargetPosition.velocity;//add ahead velocity to position.
                truncate(futureTargetPosition.agentPos, imove.max_velocity);
                UnityEngine.Debug.DrawLine(futureTargetPosition.agentPos, futureTargetPosition.targetPos, Color.red);
                steering = seek(futureTargetPosition.agentPos, futureTargetPosition.targetPos);
                truncate(steering, MAX_FORCE);
                steering *= (1 / MASS);//adjust speed.
                imove.velocity += steering;
                truncate(imove.velocity, imove.max_velocity);
                return imove.velocity;
            }

            public override Vector3 evade(AIBehaveAware imove, AIBehaveAware targetAI)
            {
                Vector3 steering = Vector3.zero;
                AIBehaveAware neighbor = targetAI;//pick up a randomizing velocity that nearly far from this fouced ai,then estimate a velocity that whether pursue target position exactly.   
                AIBehaveAware futureTargetPosition = neighbor;//travel of future forwarding target position.
                float distance = Vector3.Distance(imove.targetPos, futureTargetPosition.agentPos);
                futureTargetPosition.velocity *= (float)(Math.Truncate((distance / imove.max_velocity) * 100) / 100);//if distance / MAX_VELOCITY too short,this result very close to target position,otherwise far away from target position.
                futureTargetPosition.agentPos += futureTargetPosition.velocity;//add ahead velocity to position.
                truncate(futureTargetPosition.agentPos, imove.max_velocity);
                steering = flee(futureTargetPosition);
                truncate(steering, MAX_FORCE);
                steering *= (1 / MASS);//adjust speed.
                imove.velocity += steering;
                truncate(imove.velocity, imove.max_velocity);
                return imove.velocity;
            }

            public override Vector3 followingPath(AIBehaveAware imove, Vector3[] path)
            {
                if (path == null)
                {
                    UnityEngine.Debug.LogWarning("path could not null for " + imove.agentPos);
                    return Vector3.zero;
                }
                else
                {
                    if (imove.currentPathPoint == Vector3.zero)
                    {
                        imove.tmrTimeSpentOnPathPoint += Time.deltaTime;
                        if (!Physics.Linecast(imove.agentPos, imove.targetPos))
                        {
                            float distance = 0;
                            imove.nextNodeDistance = Vector3.Distance(imove.agentPos, path[imove.indexOfCurrentPathPoint]);
                            for (int i = 0; i < path.Length; i++)
                            {
                                distance = Vector3.Distance(imove.agentPos, path[i]);

                                if (distance < imove.nextNodeDistance)//find cloest one
                                {
                                    imove.currentPathPoint = path[i];
                                    imove.indexOfCurrentPathPoint = i;
                                    imove.nextNodeDistance = distance;
                                    imove.tmrTimeSpentOnPathPoint = 0;
                                    UnityEngine.Debug.Log("imove.indexOfCurrentPathPoint:=" + imove.indexOfCurrentPathPoint);

                                }
                            }
                        }

                    }
                    else
                    {
                        if (Vector3.Distance(imove.agentPos, imove.currentPathPoint) < MIN_DIST_TO_PATH_POINT)//choose cloest current point of path
                        {
                            UnityEngine.Debug.Log("MIN:=" + imove.indexOfCurrentPathPoint);
                            imove.indexOfCurrentPathPoint++;
                            imove.tmrTimeSpentOnPathPoint = 0;
                            if (imove.indexOfCurrentPathPoint == path.Length)
                            {
                                if (imove.loopPath)
                                {
                                    imove.indexOfCurrentPathPoint = 0;
                                }
                                else
                                {
                                    imove.indexOfCurrentPathPoint = path.Length - 1;//set back to last point of path
                                }
                            }
                            imove.currentPathPoint = path[imove.indexOfCurrentPathPoint];
                        }
                        if (imove.tmrTimeSpentOnPathPoint > MAX_TIME_TO_SPEND_ON_PATH_POINT)//spend a lot of time on the current point, maybe stuck => wander around
                        {
                            UnityEngine.Debug.Log("imove.tmrTimeSpentOnPathPoint:=" + imove.tmrTimeSpentOnPathPoint);
                            imove.tmrTimeSpentOnPathPoint = 0;
                            imove.currentPathPoint = Vector3.zero;
                        }
                        if (Vector3.Distance(imove.agentPos, imove.targetPos) < MINDISTANCETOTARGETTOSTARTSEEK)
                        {
                            //close enough to target to stop following the path and switch to seek
                            Vector3 direction = imove.targetPos - imove.agentPos;
                            UnityEngine.Debug.Log("vvvv");
                            float distance = Vector3.Distance(imove.agentPos, imove.targetPos);
                            if (distance - 15 > 0)
                            {
                                if (!Physics.Raycast(imove.agentPos, direction, distance - 15)) //minus radius of base //change this ugly solution //possible collider problem
                                {
                                    //target close enough AND visible
                                    return seek(imove.agentPos, imove.targetPos);
                                }
                                else
                                {
                                    //target close enough BUT NOT visible
                                }
                            }
                        }

                    }
                }
                iTween.MoveTo(imove.go, iTween.Hash("position", imove.currentPathPoint, "speed", 1 / MASS));
                UnityEngine.Debug.Log("------------->>>imove.currentPathPoint:=" + imove.currentPathPoint);
                return seek(imove.agentPos, imove.currentPathPoint);
            }


            public override Vector3 arrival(AIBehaveAware imove)
            {
                Vector3 steering = seek(imove.agentPos, imove.targetPos);//calculating steering
                steering -= imove.velocity;//adding force
                truncate(steering, MAX_FORCE);//adjust velocity in order to constraint this velocity
                steering *= 1 / MASS;
                imove.velocity += steering;
                truncate(imove.velocity, imove.max_velocity);
                float distance = Vector3.Distance(imove.targetPos, imove.agentPos);
                if (distance < SLOW_RADIUS)
                    imove.velocity *= (float)(Math.Truncate((distance / SLOW_RADIUS) * 100) / 100);
                return imove.velocity;

            }
            float heading;
            /// <summary>
            /// random's position on radius circumference 
            /// </summary>
            /// <param name="imove"></param>
            /// <returns></returns>
            public override Vector3 wander(AIBehaveAware imove)
            {
                Vector3 circleCenter = imove.velocity;//copy a volicty to the front circle 
                circleCenter.Normalize();//calculate volicity mangnitude 
                circleCenter.Scale(new Vector3(WANDERNG_CIRCLE_DISTANCE, imove.agentPos.y, WANDERNG_CIRCLE_DISTANCE));
                //displacement vector
                Vector3 displacementVec3 = new Vector3(1, imove.agentPos.y, -2);
                displacementVec3.Scale(new Vector3(WANDERNG_CIRCLE_RADIUS, imove.agentPos.y, WANDERNG_CIRCLE_RADIUS));
                var floor = Mathf.Clamp(imove.rotation.eulerAngles.y - WANDERING_ANGLE_CHANGE, 0, 360);
                var ceil = Mathf.Clamp(imove.rotation.eulerAngles.y + WANDERING_ANGLE_CHANGE, 0, 360);
                heading = Random.Range(floor, ceil);
                displacementVec3 = new Vector3(0, Quaternion.Euler(0, heading, 0).eulerAngles.y, 0);

                circleCenter.Scale(displacementVec3);//wander force
                Vector3 wanderForce = circleCenter;
                truncate(wanderForce, MAX_FORCE);
                wanderForce *= 1 / MASS;
                imove.velocity += wanderForce;
                UnityEngine.Debug.DrawLine(imove.agentPos, wanderForce, Color.red);
                truncate(imove.velocity, MAX_VELOCITY);
                return imove.velocity;
            }

            /// <summary>
            /// find nearest obstacle in obstacles,and calculating distance between this obstacle centen and boid position,adjudge whether this distance greater or equal collision radius length,if greater,then not collide currently,otherwise it's colliding.
            /// order avoid collision by current position's length/MAX_Velocity.as a consequeuence,adding the avoidance force into position.     
            /// </summary>
            /// <param name="imove"></param>
            /// <returns></returns>
            public override Vector3 collisonAvoidance(AIBehaveAware imove)
            {

                Vector3 steering = seek(imove.agentPos, imove.targetPos);
                steering += calculateAvoidance(imove);//avoidance force
                truncate(steering, MAX_FORCE);//avoid irregularity
                steering *= 1 / MASS;//different force seem likes more nature
                truncate(steering, MAX_VELOCITY);
                Vector3 avoidance = calculateAvoidance(imove);


                return Vector3.zero;
            }
            private Vector3 calculateAvoidance(AIBehaveAware target)
            {
                Vector3 avoidance = Vector3.zero;
                Vector3 velocityClone = target.velocity;
                Vector3 ahead = target.agentPos;
                velocityClone.Normalize();
                velocityClone.Scale(Vector3.one * MAX_AVOID_AHED * (Vector3.Distance(velocityClone, Vector3.zero) / MAX_VELOCITY));

                ahead += velocityClone;
                if (obstacles.Count > 0)
                {
                    Vector3 mostThreatening = obstacles[0];
                    float currDistance = 0;
                    for (int i = 0; i < obstacles.Count; i++)
                    {
                        Vector3 centerObstacle = obstacles[i];

                        bool collision = lineIntersecsCircle(target.agentPos, ahead, target.velocity, centerObstacle);
                        currDistance = Vector3.Distance(target.agentPos, centerObstacle);
                        if (collision && currDistance <= Vector3.Distance(target.agentPos, mostThreatening))//just find a nearest obstacle
                        {

                            mostThreatening = centerObstacle;
                        }
                    }
                    avoidance = ahead - mostThreatening;
                    avoidance.Normalize();
                    avoidance.Scale(Vector3.one * AVOID_FORCE);

                }

                return avoidance;
            }

            /// <summary>
            /// whehter the distance that between ahead(ahead2(half of ahead),
            /// and target boid position) and the center of a obstacle is less than or equal the distance of this obstacle radius,
            /// which means,if true,it's collision in time,if not,didn't collide currently.
            /// </summary>
            /// <param name="position"></param>
            /// <param name="ahead"></param>
            /// <param name="velocity"></param>
            /// <param name="centerObstacle"></param>
            /// <returns></returns>
            private Boolean lineIntersecsCircle(Vector3 position, Vector3 ahead, Vector3 velocity, Vector3 centerObstacle)
            {
                Vector3 velocity2 = velocity;
                velocity2.Normalize();
                velocity2.Scale(Vector3.one * MAX_AVOID_AHED * .5f * (velocity.magnitude / MAX_VELOCITY));
                Vector3 ahead2 = position + velocity2;

                return (Vector3.Distance(ahead, centerObstacle) <= MAX_OBSTACLE_RADIUS) || Vector3.Distance(ahead2, centerObstacle) <= MAX_OBSTACLE_RADIUS || Vector3.Distance(position, centerObstacle) <= MAX_OBSTACLE_RADIUS;

            }


            public override Vector3 queue(AIBehaveAware imove)
            {
                Vector3 velocityClone = imove.velocity;
                Vector3 velocity = seek(imove.agentPos, imove.targetPos);
                velocity += collisonAvoidance(imove);
                //estimate queue
                Vector3 nearestNeighbor = getNeighborAhead(imove);
                Vector3 brakeForce = Vector3.zero;
                if (nearestNeighbor != Vector3.zero)
                {
                    brakeForce = new Vector3(-velocity.x * .8f, velocity.y, -velocity.z * .8f);
                    brakeForce += velocityClone * -1;
                    brakeForce += separation(imove);
                    if (Vector3.Distance(imove.agentPos, nearestNeighbor) <= MAX_QUEUE_RADIUS)
                    {
                        velocity *= .3f;
                    }
                   
                }
                velocity += brakeForce;

                return velocity;
            }

            private Vector3 getNeighborAhead(AIBehaveAware imove)
            {
                Vector3 velocity = imove.velocity;
                velocity.Normalize();
                velocity *= MAX_QUEUE_AHEAD;
                Vector3 temp = Vector3.zero;
                Vector3 ahead = imove.agentPos + velocity;//ahead of this boid.
                //find a neareast bolt from horde of bolts.
                for (int i = 1; i < list.Count; i++)
                {
                    if (!list[i].Equals(imove) && Vector3.Distance(ahead, list[i].agentPos) <= MAX_QUEUE_RADIUS)//find a neighbour
                    {
                        temp = list[i].centerVect3;
                        break;
                    }
                }
                return temp;
            }

            public override Vector3 separation(AIBehaveAware imove)
            {
                Vector3 force = Vector3.zero;
                int neighborCount = 0;
                for (int i = 0; i < list.Count; i++)
                {
                    AIBehaveAware nextNeighbour = list[i];
                    if (!nextNeighbour.Equals(imove) && Vector3.Distance(imove.agentPos, nextNeighbour.agentPos) <= SEPARATION_RADIUS)
                    {
                        force += (nextNeighbour.agentPos - imove.agentPos);
                        neighborCount++;
                    }
                }
                if (neighborCount != 0)
                {
                    force = (force / neighborCount);
                }
                force.Normalize();
                force *= MAX_SEPARATION;
                return force;
            }

            public override Vector3 followLeader(AIBehaveAware imove, AIBehaveAware leader)
            {
                Vector3 velocityClone = imove.velocity;
                Vector3 force = Vector3.zero;
                if (imove.isLeader)
                {
                    force = seek(imove.agentPos, imove.targetPos);
#if UNITY_EDITOR
                    UnityEngine.Debug.DrawLine(imove.agentPos, imove.targetPos, Color.blue);

                    UnityEngine.Debug.Log("Leader Force:" + force);
#endif
                  
                }

                else
                {
                    if (leader != null&&!imove.Equals(leader))
                    {
                        force += calculateFollowingLeaderArriveForce(imove, leader);

                    }
                    else if (list.Count > 1)
                    {
                        force += calculateFollowingLeaderArriveForce(imove, list.Where(x => !x.Equals(imove)).FirstOrDefault());
                    }
                force += separation(imove);
                }
                //truncate(force, MAX_FORCE);
                //force *= 1 / MASS;
                truncate(force, imove.isLeader?MAX_VELOCITY:MAX_VELOCITY * (0.3f + Random.value * 0.5f));
                return force;
            }


            private Vector3 calculateFollowingLeaderArriveForce(AIBehaveAware targetPos, AIBehaveAware leader)
            {
                Vector3 force = Vector3.zero;
                Vector3 lv = leader.velocity;
                Vector3 ahead = Vector3.zero;
                Vector3 behind = Vector3.zero;
                lv.Normalize();
                lv *= LEADER_BEHIND_DIST;
                ahead = leader.agentPos + lv;
#if UNITY_EDITOR
                UnityEngine.Debug.DrawLine(leader.agentPos, ahead, Color.blue);
#endif
                lv *= -1;

                behind = leader.agentPos + lv;
#if UNITY_EDITOR
                UnityEngine.Debug.DrawLine(leader.agentPos, behind, Color.green);
#endif
                if (isOnLeaderSight(targetPos.agentPos, ahead, leader.agentPos))
                {
                    force += evade(targetPos, leader);
                    //Debug.Log("isOnLeaderSight:" + force);
#if UNITY_EDITOR
                    UnityEngine.Debug.DrawLine(targetPos.agentPos, leader.agentPos, Color.green);
#endif
                }
                //arrival force

                Vector3 arrival = ((behind - targetPos.agentPos).normalized);
                float arrivaldistance = Vector3.Distance(behind, targetPos.agentPos);
                if (arrivaldistance <= SLOWINGRADIUS)
                {
                    arrival *= MAX_VELOCITY * ((float)(Math.Truncate((arrivaldistance / SLOWINGRADIUS) * 100) / 100));
                }
                else
                {
                    arrival *= MAX_VELOCITY;
                }
#if UNITY_EDITOR
                UnityEngine.Debug.DrawLine(behind, targetPos.agentPos, Color.clear);
#endif
                truncate(arrival, MAX_FORCE);//adjust velocity in order to constraint this velocity
                arrival *= 1 / MASS;
                force += arrival;
                truncate(targetPos.velocity, MAX_VELOCITY);
                return force;
            }


            private bool isOnLeaderSight(Vector3 target, Vector3 leaderAhead, Vector3 leaderPos)
            {
                return (Vector3.Distance(target, leaderAhead) <= LEADER_SIGHT_RADIUS) || (Vector3.Distance(target, leaderPos) <= LEADER_SIGHT_RADIUS);
            }

            public Vector3 adjustVector3(AIBehaveAware imove)
            {


                return imove.agentPos;
            }
        }


    }


}